#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
#include <turtle2pose/position.h>
#include <iostream>
#include <unistd.h>

class turtlepose 
{
    public:
    turtlepose();
    private:
    ros::NodeHandle n;
    ros::Subscriber sub;
    ros::Publisher pub;
    ros::ServiceServer service;
    void callback(const turtlesim::Pose::ConstPtr& pose);//订阅回调函数
    bool callbackSrv(turtle2pose::position::Request& req,turtle2pose::position::Response& res);//服务回调函数
    float pose_x,pose_y,pose_z;//乌龟的位置
    float error_x,error_y,error_z,error,error1;
    double vl,va;
};
turtlepose::turtlepose()
{
    sub = n.subscribe<turtlesim::Pose>("/turtle1/pose",10,&turtlepose::callback,this);
    pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1);
    service = n.advertiseService("pose_srv", &turtlepose::callbackSrv, this);
}
/** 订阅乌龟的位置**/
void turtlepose::callback(const turtlesim::Pose::ConstPtr& pose)
{
    pose_x = pose->x;//将pose消息的x赋值给变量pose_x
    pose_y = pose->y;//将pose消息的y赋值给变量pose_y
    pose_z = pose->theta;//将pose消息的theta赋值给pose_z
    if(pose_z>3.14159)pose_z -= 6.28319;//角度做一个定义域处理
    if(pose_z<-3.14159)pose_z +=6.2819;
    // ROS_INFO('pose_z:%.2f',pose_z);
}

bool turtlepose::callbackSrv(turtle2pose::position::Request& req, turtle2pose::position::Response& res)//服务回调
{
    geometry_msgs::Twist v; //一个速度的消息格式，假定做匀速直线运动，设定运行时间为3s
    float goal = atan2((req.y-pose_y), (req.x-pose_x));
    // ROS_INFO("goal:%.2f", goal);
    while(1)
    {
        error_z=atan2((req.y-pose_y),(req.x-pose_x))-pose_z; 
        if(error_z>3.14159) error_z -= 6.28319;//角度做一个定义域处理
        if(error_z<-3.14159) error_z += 6.28319;
        error_x = (req.x-pose_x)*(req.x-pose_x)+(req.y-pose_y)*(req.y-pose_y);
        error_x = sqrt(error_x);//均方误差：反映估计量与被估计量差异程度
        // ROS_INFO("error_z:%.2f, x:%.2f,  y:%.2f", atan2((req.y-pose_y),(req.x-pose_x)), pose_x,);
        if(fabs(error_x)<0.05)//当差异小于0.05时，停止移动
        {
            v.angular.z = 0;
            v.linear.x = 0;
            pub.publish(v);
            break;
        }
        v.angular.z = error_z*10;
        v.linear.x = error_x*1;
        pub.publish(v);
        ros::spinOnce();
        usleep(200);
    }
    while(1)
    {
        error_z = req.th-pose_z;
        if(error_z>3.14159)error_z-=6.28319; //角度做一个定义域处理
        if(error_z<-3.14159)error_z+=6.28319;
        // ROS_INFO('error_z:%.2f', error_z);
        if(fabs(error_z)<0.05)
        {
            v.angular.z = 0;
            pub.publish(v);
            break;
        } 
        v.angular.z = error_z*1;
        pub.publish(v);
        ros::spinOnce();
        usleep(200);
    }
    // ROS_INFO('liear ok!');
    res.result=true;
    // ROS_INFO('arrvie goal!'); //打印信息
    return 1;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv,"turtle_pose");
    turtlepose z;//生成一个对象z
    ros::spin();
    return 0;
}

